#include "simple_tp.h"
#include "rtapi/rtapi_math.h"
#include "rtapi/rtapi.h"
void simple_tp_update(simple_tp_t* tp, double period)
{
    double max_dv, tiny_dp, pos_err, vel_req;
    tp->active = 0;
    /* compute max change in velocity per servo period */
    max_dv = tp->max_acc * period;
    /* compute a tiny position range, to be treated as zero */
    tiny_dp = TINY_DP(tp->max_acc, period);
    /* calculate desired velocity */
    if (tp->enable)
    {
        // pos_cmd 在被激活时，关节运动设置的target为正或负限位， 笛卡尔运动设置的target为无穷远处
        pos_err = tp->pos_cmd - tp->curr_pos;

        /* positive and negative errors require some sign flipping to
		avoid sqrt(negative) */
        if (pos_err > tiny_dp)
        {
            vel_req = -max_dv + sqrt(2.0 * tp->max_acc * pos_err + max_dv * max_dv);
            /* mark planner as active */
            tp->active = 1;
        }
        else if (pos_err < -tiny_dp)
        {
            vel_req = max_dv - sqrt(-2.0 * tp->max_acc * pos_err + max_dv * max_dv);
            /* mark planner as active */
            tp->active = 1;
        }
        else
        {
            /* within 'tiny_dp' of desired pos, no need to move */
            vel_req = 0.0;
        }
    }
    else
    {
        // 当该轴为disable时，将目标速度更新为0,使得该轴立即减速至0
        vel_req = 0.0;
        /* and set command to present position to avoid movement when
			next enabled */
        tp->pos_cmd = tp->curr_pos;
    }

    /* limit velocity request */
    if (vel_req > tp->max_vel)
    {
        vel_req = tp->max_vel;
    }
    else if (vel_req < -tp->max_vel)
    {
        vel_req = -tp->max_vel;
    }

    /* ramp velocity toward request at accel limit */
    if (vel_req > tp->curr_vel + max_dv)
    {
        tp->curr_vel += max_dv;
    }
    else if (vel_req < tp->curr_vel - max_dv)
    {
        tp->curr_vel -= max_dv;
    }
    else
    {
        tp->curr_vel = vel_req;
    }

    if (tp->curr_vel > 1e-6 || tp->curr_vel < -1e-6)
    {
        /* yes, mark planner active */
        tp->active = 1;
    }
    else
    {
        tp->curr_vel = 0;
    }

    /* integrate velocity to get new position */
    tp->curr_pos += tp->curr_vel * period;
}

void simple_tp_deceleration(simple_tp_t* tp, double period)
{
    double max_dv, tiny_dp, pos_err, vel_req;
    tp->active = 0;
    /* compute max change in velocity per servo period */
    max_dv = tp->max_acc * period;
    /* calculate desired velocity */
    if (tp->enable)
    {
        vel_req = 0.0;
    }

    /* ramp velocity toward request at accel limit */
    if (tp->curr_vel > 0)
    {
        tp->curr_vel -= max_dv;
        if (tp->curr_vel < 0)
        {
            tp->curr_vel = 0;
        }
    }
    else if (tp->curr_vel < 0)
    {
        tp->curr_vel += max_dv;
        if (tp->curr_vel > 0)
        {
            tp->curr_vel = 0;
        }
    }

    // #TODO 这里直接与0比较，可能有风险
    if (tp->curr_vel != 0.0)
    {
        /* yes, mark planner active */
        tp->active = 1;
    }

    /* integrate velocity to get new position */
    tp->curr_pos += tp->curr_vel * period;
}

void coarse_plan(double s, double a_max, double vp, double* va, double* t)
{
    // critical displacement: sc
    double sc = vp * vp / a_max;

    if (fabs(s) < sc)
    {
        *va = sqrt(fabs(s) * a_max);
        t[0] = sqrt(fabs(s) / a_max);
        t[1] = 0;
        t[2] = t[0];
    }
    else
    {
        *va = vp;
        t[0] = vp / a_max;
        t[1] = (fabs(s) - sc) / vp;
        t[2] = t[0];
    }

    // rtapi_print("coarse_plan: s = %f, va = %f, t[0] = %f, t[1] = %f, t[2]=%f\n", s, *va, t[0], t[1], t[2]);
}